#ifndef MOTORCONTROL_H
#define MOTORCONTROL_H

//All includes
#include "ros/ros.h"
#include <std_msgs/Int32.h>
#include "roboard_drivers/Encoder.h"
#include "roboard_drivers/Motor.h"
#include "saint_nr_9/MotCom.h"
#include "saint_nr_9/MotCorrect.h"
#include <unistd.h>
//#include "roboard_drivers/imu.h"
#include <stdio.h>
//#include "serializer/Serializer.cc"
#include "Serializer.hh"
#include <csignal>


//All defines
#ifndef TICKS_P_DEGREE
	#define TICKS_P_DEGREE	1.7 //1.72 before
#endif

#ifndef METERS_P_TICKS
	#define METERS_P_TICKS	0.0019
#endif

#ifndef SPEED
	#define STRAIGHT_SPEED	0.4
	#define TURN_SPEED 	0.3
#endif

enum command {MOVE_FORWARD_DIST, MOVE_BACKWARD_DIST, TURN_LEFT, TURN_RIGHT, MOVE_FORWARD, MOVE_FORWARD_CORRECT, STOP};

using namespace roboard_drivers;

//All forward decalatations
int dist(int a, int b);
void forward(double distance, int sl, int sr);
void backward(double distance, int sl, int sr);
void t_left(double angel, int sl, int sr);
void t_right(double angel, int sl, int sr);
void forward_infinity(int sl, int sr);
void forward_easy_correct(int sl, int sr);
void stop(int sl, int sr);

#endif
